#ifndef Mat3_HPP
#define Mat3_HPP
#include "Vec2.hpp"
#include "Math.hpp"
class Mat3
{
public:
   typedef float T;
   Mat3()
   {
      set(1,0,0,
          0,1,0,
          0,0,1);
   }
   static Mat3 zero()
   {
      return Mat3(0,0,0,0,0,0,0,0,0);
   }

   Mat3(T a, T b, T c,
        T d, T e, T f,
        T g, T h, T i)
   {
      set(a,b,c,d,e,f,g,h,i);
   }
    void set(T a, T b, T c,
             T d, T e, T f,
             T g, T h, T i)
    {
       values[0] = a;values[1] = b; values[2] = c;values[3] = d;
       values[4] = e;values[5] = f; values[6] = g;values[7] = h;
       values[8] = i;
    }

   T& operator()(int a, int b)
   {
      return values[3 * a + b];
   }
   T operator()(int a, int b) const
   {
      return values[3 * a + b];
   }

   Mat3 transpose() const
   {
      return Mat3(values[0], values[3], values[6],
                  values[1], values[4], values[7],
                  values[2], values[5], values[8]);
   }
   T det() const
   {
      const T& a = values[0]; const T& b = values[1]; const T& c = values[2];
      const T& d = values[3]; const T& e = values[4]; const T& f = values[5];
      const T& g = values[6]; const T& h = values[7]; const T& i = values[8];

      return (a*e*i + b*f*g + c*d*h) - (g*e*c + h*f*a + i*d*b);
   }
   Mat3 operator+(Mat3 a)
   {
      return Mat3(values[0] + a.values[0], values[1] + a.values[1], values[2] + a.values[2],
                  values[3] + a.values[3], values[4] + a.values[4], values[5] + a.values[5],
                  values[6] + a.values[6], values[7] + a.values[7], values[8] + a.values[8]);
   }
   Mat3 operator-(Mat3 a)
   {
      return Mat3(values[0] - a.values[0], values[1] - a.values[1], values[2] - a.values[2],
                  values[3] - a.values[3], values[4] - a.values[4], values[5] - a.values[5],
                  values[6] - a.values[6], values[7] - a.values[7], values[8] - a.values[8]);
   }
   Mat3& operator*=(T a)
   {
      for (int i = 0; i < 9; ++i) values[i] *= a;
      return *this;
   }
   Mat3& operator/=(T a)
   {
      for (int i = 0; i < 9; ++i) values[i] /= a;
      return *this;
   }
   Mat3& operator+=(T a)
   {
      for (int i = 0; i < 9; ++i) values[i] += a;
      return *this;
   }
   Mat3& operator-=(T a)
   {
      for (int i = 0; i < 9; ++i) values[i] -= a;
      return *this;
   }
   Mat3 operator-() const
   {
      Mat3 r(*this);
      for (int i = 0; i < 9; ++i) r.values[i] = -r.values[i];
      return r;
   }
private:
   static T det2(float a, float b, float c, float d) { return a * d - b * c; }
public:
   Mat3 inverse() const
   {
      const T& a11 = values[0]; const T& a12 = values[1]; const T& a13 = values[2];
      const T& a21 = values[3]; const T& a22 = values[4]; const T& a23 = values[5];
      const T& a31 = values[6]; const T& a32 = values[7]; const T& a33 = values[8];
      Mat3 tmp( det2(a22, a23, a32, a33), det2(a13, a12, a33, a32), det2(a12, a13, a22, a23),
                det2(a23, a21, a33, a31), det2(a11, a13, a31, a33), det2(a13, a11, a23, a21),
                det2(a21, a22, a31, a32), det2(a12, a11, a32, a31), det2(a11, a12, a21, a22));
      tmp *= T(1.0) / det();
      return tmp;
   }


   Mat3 operator*(T b) const
   {
      Mat3 r(*this);
      r *= b;
      return r;
   }

   Mat3 operator/(T b) const
   {
      Mat3 r(*this);
      r /= b;
      return r;
   }
   Mat3 operator*=(const Mat3& b)
   {
      *this = *this * b;
      return *this;
   }
   Mat3 operator*(const Mat3& b) const
   {
      return Mat3(
        values[0]*b.values[0] +
        values[1]*b.values[3] +
        values[2]*b.values[6],

        values[0]*b.values[1] +
        values[1]*b.values[4] +
        values[2]*b.values[7],

        values[0]*b.values[2] +
        values[1]*b.values[5] +
        values[2]*b.values[8],

        values[3]*b.values[0] +
        values[4]*b.values[3] +
        values[5]*b.values[6],

        values[3]*b.values[1] +
        values[4]*b.values[4] +
        values[5]*b.values[7],

        values[3]*b.values[2] +
        values[4]*b.values[5] +
        values[5]*b.values[8],

        values[6]*b.values[0] +
        values[7]*b.values[3] +
        values[8]*b.values[6],

        values[6]*b.values[1] +
        values[7]*b.values[4] +
        values[8]*b.values[7],

        values[6]*b.values[2] +
        values[7]*b.values[5] +
        values[8]*b.values[8]);
   }

   T values[16];
};

// Point Multiplication
inline Vec2 operator*(const Vec2& a, const Mat3& b)
{
   float z = a.x*b(0,2)+a.y*b(1,2)+b(2,2);

   return Vec2(
   (a.x*b(0,0)+a.y*b(1,0)+b(2,0)) / z,
      (a.x*b(0,1)+a.y*b(1,1)+b(2,1)) / z);
}

// Vector Multiplication
inline Vec2 operator%(const Vec2& a, const Mat3& b)
{
   return Vec2(a.x*b(0,0)+a.y*b(1,0), a.x*b(0,1)+a.y*b(1,1));
}

namespace math
{
inline Mat3 rotation(float a)
{
   return Mat3(cos(a), -sin(a), 0,
               sin(a),  cos(a), 0,
               0,            0, 1);
}
inline Mat3 translate(float x, float y)
{
   return Mat3(1, 0, 0,
               0, 1, 0,
               x, y, 1);
}
inline Mat3 translate(const Vec2& a) { return translate(a.x, a.y); }
inline Mat3 scale(float x, float y, float z=1.0f)
{
   return Mat3(x, 0, 0,
               0, y, 0,
               0, 0, 1);
}
inline Mat3 scale(const Vec2& a) { return scale(a.x, a.y); }
}
#endif